/*
 * (C) Copyright 2005 Davide Brugali, Marco Torchiano
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
 * 02111-1307  USA
 */

package moro;

/**
 * Title:        The MObile RObot Simulation Environment
 * Description:  The Controller class controls the robot
 * Copyright:    Copyright (c) 2002
 * Company:      Universit� di Bergamo
 * @author       Davide Brugali
 * @version 1.0
 */

import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.PipedInputStream;
import java.io.PipedOutputStream;
import java.io.PrintWriter;
import java.util.StringTokenizer;
public class Controller implements Runnable{
	Robot robot = null;
	OccupancyMap map = new OccupancyMap();
	
	private final static int widthOfVehicle= 20;
	private final static int NORTH 	= 	270,			
			EAST 					= 	0,			
			SOUTH 					= 	90,			
			WEST 					=	180;			
	private int[] pq = new int[1];
	
	public Controller(Robot robot) {
		this.robot = robot;
		if(robot == null)
			System.exit(1);
	}

	public void start() {
		robot.start();
		// This will cause the controllers run() to be started in a separate thread.
		new Thread(this).start();
	}

	public void quit() {
	}

	public void run() {
		String result = "";
		double position[] = new double[3];
		double measures[]  = new double[360];				

		try {
			PipedInputStream pipeIn = new PipedInputStream();
			BufferedReader input = new BufferedReader(new InputStreamReader(pipeIn));
			PrintWriter output = new PrintWriter(new PipedOutputStream(pipeIn), true);

			robot.setOutput(output);
			
			System.out.println(map.isFullyDiscovered());
			
			robot.sendCommand("R1.GETPOS");
			result = input.readLine();
			parsePosition(result, position);
			
			robot.sendCommand("L1.SCAN");
			result = input.readLine();
			parseMeasures(result, measures);
			map.drawLaserScan(position, measures);
			
			while(maxFw(position) > 0){
				robot.sendCommand("R1.GETPOS");
				result = input.readLine();
				parsePosition(result, position);
				
				robot.sendCommand("L1.SCAN");
				result = input.readLine();
				parseMeasures(result, measures);
				map.drawLaserScan(position, measures);
				
				robot.sendCommand("P1.MOVEFW " + maxFw(position));	
				result = input.readLine();
			}
			
			
			robot.sendCommand("P1.ROTATELEFT 90");
			result = input.readLine();
			
			
			while (!map.isFullyDiscovered()) {				
				
				robot.sendCommand("R1.GETPOS");
				result = input.readLine();
				parsePosition(result, position);
				
				robot.sendCommand("L1.SCAN");
				result = input.readLine();
				parseMeasures(result, measures);
				map.drawLaserScan(position, measures);
				
				if(!hasRightWall(position)){
					//Go right
					robot.sendCommand("P1.ROTATERIGHT 90");
					result= input.readLine();
					robot.sendCommand("R1.GETPOS");
					result = input.readLine();
					parsePosition(result, position);
					//int maxFw = maxFw(position);
					robot.sendCommand("P1.MOVEFW 10");							
				}
				else if(maxFw(position)>0){					
					//Move forward		
					robot.sendCommand("P1.MOVEFW " + maxMoveOrRightWall(position));														
				}
				else if(!hasLeftWall(position)){
					//Go left
					robot.sendCommand("P1.ROTATELEFT 90");
				}
				else{
					//Turn around
					robot.sendCommand("P1.ROTATELEFT 180");
				}				
				result = input.readLine();				
			}
			
			robot.sendCommand("R1.GETPOS");
			result = input.readLine();
			parsePosition(result, position);
			
			//Turn around
			robot.sendCommand("P1.ROTATELEFT 180");			
		}

		catch(IOException ioe) {
			ioe.printStackTrace();
		}
	}	

	private void parsePosition(String value, double position[]) {
		int indexInit, indexEnd;
		String parameter;
		indexInit = value.indexOf("X=");
		parameter = value.substring(indexInit+2);
		indexEnd = parameter.indexOf(' ');
		position[0] = Math.round(Double.parseDouble(parameter.substring(0, indexEnd)));

		indexInit = value.indexOf("Y=");
		parameter = value.substring(indexInit+2);
		indexEnd = parameter.indexOf(' ');
		position[1] = Math.round(Double.parseDouble(parameter.substring(0, indexEnd)));

		indexInit = value.indexOf("DIR=");
		parameter = value.substring(indexInit+4);
		position[2] = Math.round(Double.parseDouble(parameter));
		//System.out.println(position[0]);
		//System.out.println(position[1]);

	}

	private void parseMeasures(String value, double measures[]) {
		for(int i=0; i < 360; i++)
			measures[i] = 100.0;
		if(value.length() < 5)
			return;
		value = value.substring(5);  // removes the "SCAN " keyword
		StringTokenizer tokenizer = new StringTokenizer(value, " ");
		double dist;
		int dir;
		
		while(tokenizer.hasMoreTokens()) {
			dist = Double.parseDouble(tokenizer.nextToken().substring(2));
			dir  = (int) Math.round( Math.toDegrees(Double.parseDouble(tokenizer.nextToken().substring(2))) );
			if(dir == 360)
				dir = 0;
			measures[dir] = dist;
			//System.out.println("Richting: " + ((int) dir) + " Afstand: " + dist);
		}
	}
	
	//In deze functie wordt gekeken hoever je vooruit kunt gaan zonder dat je ergens tegenaan rijd
	private int maxFw(double[] position) {
		int direction = toDegrees(position[2]);
		int[] xy = new int[2];
		switch(chooseDirection(position)){
			//NORTH
			case 0:
				xy[0] = (int)position[0]+20;
				xy[1] = (int)position[1]-20;
			break;
			//EAST
			case 1 :
				xy[0] = (int)position[0]+20;
				xy[1] = (int)position[1]+20;
			break;
			//SOUTH
			case 2 :
				xy[0] = (int)position[0]-20;
				xy[1] = (int)position[1]+20;
			break;
			//WEST
			case 3 : 
				xy[0] = (int)position[0]-20;
				xy[1] = (int)position[1]-20;
			break;
		}
		//int[] xy = { (int)Math.round(position[0]), (int)Math.round(position[1]) };
		//System.out.println(xy[0]+", "+ xy[1]);
		//System.out.println(position[2]);
		int distance = map.distanceToWall(xy, direction);
		//System.out.println(distance);
		if(distance > 100)distance = 100;
		return distance;
	}
	
	//In deze functie wordt gekeken of er aan de linkerkant een muur is
	private Boolean hasLeftWall(double[] position) {
		Boolean hasLeftWall = false;
		int direction = toDegrees(position[2]-90);		
		int[] xy = {(int)Math.round(position[0]), (int)Math.round(position[1]) };
		int distance = map.distanceToWall(xy, direction);
		if(distance >= 0 && distance <= 20){
			hasLeftWall = true;
		}
		return hasLeftWall;
	}
	public Boolean hasRightWall(double[] position){		
		int[] pq = {(int)position[0], (int)position[1]};	
		int direction = toDegrees(position[2]+90);
		switch(chooseDirection(position)){
			//NORTH
			case 0:
				pq[1] += 20;				
				for(int i = 0; i<=40; i+=20){
					int[] xy = {pq[0],(pq[1]-i)};
					if(isRightWall(xy, direction)) return true;		
				}
			break;
			
			//EAST	
			case 1:
				pq[0] -= 20;				
				for(int i = 0; i<=40; i+=20){					
					int[] xy = {(pq[0]+i),pq[1]};
					if(isRightWall(xy, direction)) return true;		
				}
			break;
			
			//SOUTH
			case 2:
				pq[1] -= 20;				
				for(int i = 0; i<=40; i+=20){
					int[] xy = {pq[0],(pq[1]+i)};
					if(isRightWall(xy, direction)) return true;					
				}
			break;
			
			//WEST
			case 3:
				pq[0] += 20;				
				for(int i = 0; i<=40; i+=20){
					int[] xy = {(pq[0]-i),pq[1]};
					if(isRightWall(xy, direction)) return true;		
				}
			break;			
		}	
		return false;
	}	
	
	//Deze functie wordt gebruik door hasRigthWall om te kijken of die er zit op de meegegeven coordinaten
	private Boolean isRightWall(int[] xy, int direction){
		int distanceToWall = map.distanceToWall(xy, direction);
		if(distanceToWall > 0 && distanceToWall <=20){
			//System.out.println("Op " + direction + "is een rechtermuur gevonden");
			this.pq = xy;
			return true;
		}		
		return false;
	}
	
	//Deze functie kijkt of het logischer is om de rechtermuur te volgen of gewoon de maximale voorwaartse afstand te nemen
	private int maxMoveOrRightWall(double[] position){
		int maxFw = maxFw(position);
		int lengthRightWall = 0;
		if(hasRightWall(position)){		
			lengthRightWall = lengthRightWall(position);						
		}
		else{
			lengthRightWall = 90;
		}		
		if(lengthRightWall<maxFw){
			System.out.println("Lengte rechter muur: "+lengthRightWall);
			return lengthRightWall;
		}	
		System.out.println("Maximaal vooruit: "+maxFw);
		return maxFw; 
	}
	
	//Deze functie zet het aantal graden goed
	private int toDegrees(Double dir) { 
		double direction = dir;
		if(Math.round( direction ) >= 360){
			direction -= 360;
		}
		else if(direction < 0){
			direction += 360;
		}
		return (int) direction;
	}
	
	public int lengthRightWall(double[] position){
		int lengthRightWall = 0;
		int cellDim = map.cellDim;
		
		int[] xy = {(int) position[0], (int)position[1]};
		
		int difX = 0;
		int difY = 0;
		
		//System.out.println("Difx : "+difX);
		//System.out.println("Dify : "+difY);
		int direction = toDegrees(position[2]+90);
		
		int distanceToWall = map.distanceToWall(this.pq, direction);
		
		switch(chooseDirection(position)){
			//NORTH
			case 0:	
				difY = (int)position[1] - this.pq[1];
				for(int i = difY; i<=100; i+= 10){
					int[] rt = {xy[0], xy[1]-i}; 
					int distance = map.distanceToWall(rt, direction);
					if(distance == distanceToWall){
						lengthRightWall += cellDim;
					}
					else{
						return lengthRightWall;
					}
				}
			break;
			
			//EAST
			case 1:
				difX =this.pq[0] - (int)position[0];
				for(int i = difX; i<=100; i+= 10){
					int[] rt = {xy[0]+i, xy[1]}; 
					int distance = map.distanceToWall(rt, direction);
					if(distance == distanceToWall){
						lengthRightWall += cellDim;
					}
					else{
						return lengthRightWall;
					}
				}				
				
			break;
			
			//SOUTH
			case 2:
				difY =this.pq[1] - (int)position[1];
				for(int i = difY; i<=100; i+= 10){
					int[] rt = {xy[0], xy[1]+i}; 
					int distance = map.distanceToWall(rt, direction);
					if(distance == distanceToWall){
						lengthRightWall += cellDim;						
					}
					else{
						return lengthRightWall;
					}
				}
			break;
			
			//WEST
			case 3:
				difX = (int)position[0] - this.pq[0];
				for(int i = difX; i<=100; i+= 10){
					int[] rt = {xy[0]-i, xy[1]};					
					int distance = map.distanceToWall(rt, direction);
					if(distance == distanceToWall){
						lengthRightWall += cellDim;
					}
					else{
						return lengthRightWall;
					}
				}
			break;
		}
		this.pq[0] = 0;
		this.pq[1] = 0;	
		//System.out.println(lengthRightWall);
		return lengthRightWall;
	}
	
//	//Deze functie bepaalt de lengte van de rechter muur
//	public int lengthRightWall(double[] position){		
//		int lengthRightWall = 0;
//		double[] positions = position;
//		int[] xy = {(int)positions[0],(int)positions[1]};
//		int direction = ((int)positions[2]+90);
//				
//		if(direction>=360)direction-=360;
//		int distanceToRightWall = map.distanceToWall(xy, direction);
//		
//		Boolean isTrue = true;
//		int[] pq = {(int)position[0], (int)position[1]};
//		int facingDirection = chooseDirection(position);
//		
//		//Eerst moeten de coordinaten gelijk staan met de onderkant van de robot
//		//Dit is afhankelijk van de richting
//		switch(facingDirection){
//			case 0:
//				pq[0] = (int)position[0];
//				pq[1] = (int)(position[1] + 20);
//			break;
//			case 1:
//				pq[0] = (int)(position[0] - 20);
//				pq[1] = (int)position[1];
//			break;
//			case 2:
//				pq[0] = (int)position[0];
//				pq[1] = (int)(position[1] - 20);
//			break;
//			case 3:
//				pq[0] = (int)(position[0] + 20);
//				pq[1] = (int)position[1];
//			break;
//		}
//
//		while(isTrue && lengthRightWall<=100){	
//			facingDirection = chooseDirection(position);
//			
//			switch(facingDirection){
//				case 0: pq[1] -= map.cellDim;
//				break;
//				case 1: pq[0] += map.cellDim;
//				break;
//				case 2: pq[1] += map.cellDim;
//				break;			
//				case 3: pq[0] -= map.cellDim;
//				break;
//			}
//			int distanceToWall = map.distanceToWall(pq,direction);
//			
//			if(distanceToWall > distanceToRightWall-1 && distanceToWall < distanceToRightWall+1){
//				lengthRightWall += map.cellDim;				
//			}
//			else{
//				//lengthRightWall -= map.cellDim;	
//				isTrue = false;
//			}			
//		}		
//		return lengthRightWall;
//	}
	
	private int chooseDirection(double[] position){
		int direction = toDegrees(position[2]);
		if(direction == NORTH){
			direction = 0;		
		}
		else if(direction == EAST){
			direction = 1;
		}		
		else if(direction == SOUTH){
			direction = 2;
		}
		else if(direction == WEST){
			direction = 3;
		}
		return direction;
	}
}
